#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  Sonar,          sensorSONAR_cm)
#pragma config(Sensor, dgtl7,  bumper1,        sensorTouch)
#pragma config(Sensor, dgtl8,  bumper2,        sensorTouch)
#pragma config(Motor,  port2,           LM,            tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port3,           RM,            tmotorVex269, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void forward(int revs){
 int turns=revs*360;
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
 while(SensorValue[rightEncoder]<turns){
  if(SensorValue[rightEncoder]>-SensorValue[leftEncoder]){
    motor[RM]=110;
    motor[LM]=127;
}
  else if(-SensorValue[leftEncoder]<SensorValue[rightEncoder]){
    motor[LM]=127;
    motor[RM]=110;
}
    else{
    motor[RM]=127;
    motor[LM]=127;
  }
 }
}

void backwards(int turns){
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
 while(SensorValue[rightEncoder]>-turns){
  if(-SensorValue[rightEncoder]>SensorValue[leftEncoder]){
    motor[RM]=-117;
    motor[LM]=-127;
}
  else if(SensorValue[leftEncoder]<turns){
    motor[LM]=-127;
    motor[RM]=-117;
}
    else{
    motor[RM]=-127;
    motor[LM]=-127;
  }
 }
}

void turnLeft(){
  SensorValue[leftEncoder]=0;
  SensorValue[rightEncoder]=0;
 while(SensorValue[rightEncoder]>-140){
   motor[LM]=-127;
   motor[RM]=127;
 }
}

void turnRight(){
  SensorValue[leftEncoder]=0;
  SensorValue[rightEncoder]=0;
 while(SensorValue[rightEncoder]>-100){
   motor[LM]=110;
   motor[RM]=-127;
 }
}
void pause(int wait){
  motor[RM]=0;
  motor[LM]=0;
  wait1Msec(wait);
}

task main(){
while(1==1){
  while(SensorValue[Sonar]<10){
 if(SensorValue[bumper1]==0 && SensorValue[bumper2]==0){
  forward(1);
}
 else if(SensorValue[bumper1]==1 || SensorValue[bumper2]==1){
  backwards(90);
  pause(250);
  turnLeft();
}
}
motor[LM]=100;
motor[RM]=-127;
wait1Msec(250);
forward(2);
pause(250);
}
}
